
/************************************************************************************************
 * 程序版本：V3.0
 * 程序日期：2022-11-3
 * 程序作者：719飞行器实验室
 ************************************************************************************************/
#include "main.h"

/*********************************************************************************************************
 * 函  数：void IMUupdate(FLOAT_XYZ *Gyr_filt,FLOAT_XYZ *Acc_filt,FLOAT_ANGLE *Att_Angle)
 * 功　能：获取姿态角
 * 参  数：Gyr_filt 	指向角速度的指针（注意单位必须是弧度）
 *         Acc_filt 	指向加速度的指针
 *         Att_Angle 指向姿态角的指针
 * 返回值：无
 * 备  注：求解四元数和欧拉角都在此函数中完成
 **********************************************************************************************************/
// kp=ki=0 就是完全相信陀螺仪
// kp加快响应，ki减小漂移
#define Kp 1.4                         // 1.4
                                       // 比例增益控制加速度计，磁力计的收敛速率
#define Ki 0.005                       // 0.005
                                       //  积分增益控制陀螺偏差的收敛速度
#define halfT 0.0025f                  // half the sample period 采样周期的一半
#define yawT 0.006926                  // 积分采样时间 越大，积分数值越大
float q0 = 1, q1 = 0, q2 = 0, q3 = 0;  // quaternion elements representing the estimated orientation
float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error

// void IMUupdate(FLOAT_XYZ *Gyr_filt, FLOAT_XYZ *Acc_filt, FLOAT_ANGLE *Att_Angle)
void attitude(imu963_t *imu)
{

    float ax = imu->acc_real[0], ay = imu->acc_real[1], az = imu->acc_real[2];
    float gx = imu->gyro_real_sec[0], gy = imu->gyro_real_sec[1], gz = imu->gyro_real_sec[2];
    float vx, vy, vz;
    float ex, ey, ez;
    float norm;

    float q0q0 = q0 * q0;
    float q0q1 = q0 * q1;
    float q0q2 = q0 * q2;
    float q0q3 = q0 * q3;
    float q1q1 = q1 * q1;
    float q1q2 = q1 * q2;
    float q1q3 = q1 * q3;
    float q2q2 = q2 * q2;
    float q2q3 = q2 * q3;
    float q3q3 = q3 * q3;

    if (ax * ay * az == 0)
        return;

    // 加速度计<测量>的重力加速度向量(机体坐标系)
    norm = invSqrt(ax * ax + ay * ay + az * az);
    ax = ax * norm;
    ay = ay * norm;
    az = az * norm;
    //	printf("ax=%0.2f ay=%0.2f az=%0.2f\r\n",ax,ay,az);

    // 陀螺仪积分<估计>重力向量(机体坐标系)
    vx = 2 * (q1q3 - q0q2);         // 矩阵(3,1)项
    vy = 2 * (q0q1 + q2q3);         // 矩阵(3,2)项
    vz = q0q0 - q1q1 - q2q2 + q3q3; // 矩阵(3,3)项

    // printf("vx=%0.2f vy=%0.2f vz=%0.2f\r\n",vx,vy,vz);

    // 向量叉乘所得的值
    ex = (ay * vz - az * vy);
    ey = (az * vx - ax * vz);
    ez = (ax * vy - ay * vx);

    // 用上面求出误差进行积分
    exInt = exInt + ex * Ki;
    eyInt = eyInt + ey * Ki;
    ezInt = ezInt + ez * Ki;

    // 将误差PI后补偿到陀螺仪
    gx = gx + Kp * ex + exInt;
    gy = gy + Kp * ey + eyInt;
    gz = gz + Kp * ez + ezInt; // 这里的gz由于没有观测者进行矫正会产生漂移，表现出来的就是积分自增或自减

    // 四元素的微分方程
    q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT;
    q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT;
    q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT;
    q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT;

    // 单位化四元数
    norm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 = q0 * norm;
    q1 = q1 * norm;
    q2 = q2 * norm;
    q3 = q3 * norm;

    //  矩阵表达式
    //	matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;	 // 11
    //	matrix[1] = 2.f * (q1q2 + q0q3);	       // 12
    //	matrix[2] = 2.f * (q1q3 - q0q2);	       // 13
    //	matrix[3] = 2.f * (q1q2 - q0q3);	       // 21
    //	matrix[4] = q0q0 - q1q1 + q2q2 - q3q3;	 // 22
    //	matrix[5] = 2.f * (q2q3 + q0q1);	       // 23
    //	matrix[6] = 2.f * (q1q3 + q0q2);	       // 31
    //	matrix[7] = 2.f * (q2q3 - q0q1);	       // 32
    //	matrix[8] = q0q0 - q1q1 - q2q2 + q3q3;	 // 33

    // 四元数转换成欧拉角(Z->Y->X)

    // 偏航角YAW
    if ((imu->gyro_real_sec[2] * RadtoDeg > 1.0f) || (imu->gyro_real_sec[2] * RadtoDeg < -1.0f)) // 数据太小可以认为是干扰，不是偏航动作
    {
        imu->yaw += imu->gyro_real_sec[2] * RadtoDeg * yawT;
    }
    // if ((gz * RadtoDeg > 1.0f) || (gz * RadtoDeg < -1.0f)) // 数据太小可以认为是干扰，不是偏航动作
    // {
    //     imu->yaw += gz * RadtoDeg * yawT;
    // }

    if (imu->yaw > 179.0f)
    {
        imu->yaw -= 360.0f; // 修正为减去 360
    }
    if (imu->yaw < -179.0f)
    {
        imu->yaw += 360.0f; // 修正为加上 360
    }

    // const float yaw_factor = 0.51;
    // static float yaw_buf;
    // // 滤波因子越小，响应速度越慢，平滑效果越好
    // imu->yaw_temp = yaw_buf = yaw_buf * (1 - yaw_factor) + imu->yaw_temp * yaw_factor;
    // if (imu->yaw > 179.0f)
    // {
    //     imu->yaw -= 360.0f; // 修正为减去 360
    // }
    // if (imu->yaw < -179.0f)
    // {
    //     imu->yaw += 360.0f; // 修正为加上 360
    // }

    //  hhSerial_Printf("%f,%f,%f\r\n", imu->yaw, imu->pitch, imu->roll); // YAW角数据（非常稳定）

    // 横滚角ROLL
    // imu->roll = -asin(2.f * (q1q3 - q0q2)) * 57.3f;
    imu->roll = -myAsin(2.f * (q1q3 - q0q2)) * 57.3f;

    //  printf("%f\r\n",Att_Angle->rol);//ROLL角数据

    // 俯仰角PITCH
    // imu->pitch = -atan2(2.f * q2q3 + 2.f * q0q1, q0q0 - q1q1 - q2q2 + q3q3) * 57.3f;
    imu->pitch = -myAtan2(2.f * q2q3 + 2.f * q0q1, q0q0 - q1q1 - q2q2 + q3q3) * 57.3f;
    //	printf("pitch: %f\r\n",Att_Angle->pit);//PITCH角数据
    //    printf("%f,%f,%f\r\n",Att_Angle->pit,Att_Angle->rol,Att_Angle->yaw);//PITCH角数据
}

// kaerman
void kalman_1(struct _1_ekf_filter *ekf, float input) // 一阶卡尔曼
{
    ekf->Now_P = ekf->LastP + ekf->Q;
    ekf->Kg = ekf->Now_P / (ekf->Now_P + ekf->R);
    ekf->out = ekf->out + ekf->Kg * (input - ekf->out);
    ekf->LastP = (1 - ekf->Kg) * ekf->Now_P;
}